#include "inc/hw_types.h"		// tBoolean
#include "RASLib/init.h"
#include "RASLib/motor.h"
#include "RASLib/uart.h"
#include "driverlib/adc.h"
#include "xor-0.h"
#include "RASLib/encoder.h"
#include "RASLib/timer.h"
#include "pid.h"

#include "RASLib/linesensor.h"

#define FRONT 0
#define SIDE 2
#define LEFT 0
#define RIGHT 1

int destination(void);
long sample(int);
void control(long, int, int[]);
void wallFollow(power_t velocity);
power_t compAdjust(void);
void lineFollow(power_t velocity);
power_t compAdjust2(void);

void quicktrip(void) {
		//long init_value;
		//int motor_power[2];	  //0 is left, 1 is right
 		InitializeMotors(false, true);
		initIRSensor();
		initLineSensor();
		/*init_value = getADCValue(SIDE);
		motor_power[LEFT] = 127;
		motor_power[RIGHT] = 127;*/
	while(true){
		while (!destination()){
			  //control(init_value, 1, motor_power);
			  lineFollow(110);
			  Wait(100);
			  
			  /*long dist = sample(SIDE);	 //(right, left)
			  if(dist >800) SetMotorPowers(110, 127);  //if too close, turn right
			  else if (dist < 800) SetMotorPowers(127, 110); //if too far, turn left
			  else 	SetMotorPowers(116,110);   */
			  //if
		}
		SetMotorPowers(-128, 127);		 //FOR QUICKTRIP
		Wait(200);
	}
		/*while(1) {
			control(init_value, 0, motor_power);
		}*/
}

void lineFollow(power_t velocity) {		
		power_t diff = 0;
		power_t left, right;
		diff = compAdjust2();

PRINT("LINEFOLLOW"); NL;

		if(velocity < 0) {
			diff = -diff;
		}
		PRINT_I(diff); NL;
		left = SATURATE(velocity + diff, -128, 127);
		right = SATURATE(velocity - diff, -128, 127);
		PRINT_D(left); TAB; PRINT_D(right);
		SetMotorPowers(right, left);
}

void wallFollow(power_t velocity) {		
		power_t diff = 0;
		power_t left, right;
 		InitializeMotors(false, true);
		initIRSensor();
		diff = compAdjust();
		if(velocity < 0) {
			diff = -diff;
		}
		PRINT_I(diff); NL;
		left = SATURATE(velocity + 2*diff, -128, 127);
		right = SATURATE(velocity - diff, -128, 127);
		PRINT_D(left); TAB; PRINT_D(right);
		SetMotorPowers(right, left);
}

power_t compAdjust2(){
	float adjustment = 0;	
	float position = 0.0;		 
	int i;
	float left = -1;
	float right = -1;
	unsigned char lineSensorByte;

	static SPid pid = {
		/*.dState = */0.0f,
		/*.iState = */-0.0f,
		/*.iMax = */10.0f,
		/*.iMin = */-10.0f,
		/*.pGain = */12.0f,
		/*.iGain = */0.00f,
		/*.dGain = */0.00f
	};
	
	signed long dist = ReadLineSensor();
	for(i = 0; i < 8; i++) {
		if ((lineSensorByte&0x01)) {
			
			if(left == -1)left = i;
			right = i;
		}
		lineSensorByte = lineSensorByte >> 1;
	}
	position = (left+right)*0.5;
	PRINT_D((int)left); TAB;
	PRINT_D((int)right); NL;
	adjustment = UpdatePID(&pid,position-3.5,position);
	adjustment = SATURATE(adjustment, 17, -17);

	return (power_t)adjustment;
}

power_t compAdjust(){
	float adjustment = 0;

	static SPid pid = {
		/*.dState = */0.0f,
		/*.iState = */-0.0f,
		/*.iMax = */10.0f,
		/*.iMin = */-10.0f,
		/*.pGain = */0.12f,
		/*.iGain = */0.002f,
		/*.dGain = */0.00f
	};
	
	signed long dist = sample(SIDE);	 //(right, left)
	signed long err = dist-600;
	adjustment = UpdatePID(&pid,(float)err,(float)dist);
	adjustment = SATURATE(adjustment, 17, -17);

	return (power_t)adjustment;
}


int destination(void) {
	long s = sample(FRONT);
	//PRINT_D(s); NL;
 	return s > 700;
}

long sample(int ADCnumb) {
  	long value[3] = {0};
	int i;
	for(i = 0; i < 3; ++i) {
		value[i] = getADCValue(ADCnumb);
		//PRINT_I(i); PRINT(" "); 
		//PRINT_D(value[i]); TAB;
	}
	//PRINT_D(value[0]); NL;
	return SATURATE(value[0], value[1], value[2]);
}
/*
void control(long init_value, int forward, int m_power[]) {
	long curr_value = sample(SIDE);
	PRINT_D(curr_value);
 	if(forward == 1) {
	 	if(curr_value > init_value + 10) {
			m_power[LEFT] = (int)(m_power[LEFT] * .95);
			m_power[RIGHT] = 127;
		}
		else if(curr_value < init_value - 10) {
			m_power[LEFT] = 127;
			m_power[RIGHT] = (int)(m_power[RIGHT] * .95);
		}
	}
	else {
	   	 if(curr_value > init_value - 10) {
			m_power[LEFT] = (int)(m_power[LEFT] * .95);
			m_power[RIGHT] = -128;
		}
		else if(curr_value < init_value + 10) {
			m_power[LEFT] = -128;
			m_power[RIGHT] = (int)(m_power[RIGHT] * .95);
		}
	}
	SetMotorPower(m_power[LEFT], m_power[RIGHT]);
}*/
